Navigation and control system for autonomous vehicles

ABSTRACT

A navigation and control system including a sensor configured to locate objects in a predetermined field of view from a vehicle. The sensor has an emitter configured to repeatedly scan a beam into a two-dimensional sector of a plane defined with respect to a first predetermined axis of the vehicle, and a detector configured to detect a reflection of the emitted beam from one of the objects. The sensor includes a panning mechanism configured to pan the plane in which the beam is scanned about a second predetermined axis to produce a three dimensional field of view. The navigation and control system includes a processor configured to determine the existence and location of the objects in the three dimensional field of view based on a position of the vehicle and a time between an emittance of the beam and a reception of the reflection of the emitted beam from one of the objects.

CROSS REFERENCE TO RELATED APPLICATIONS

This application is a continuation of Ser. No. 13/240,576 filed Sep. 22,2011 which is a continuation of U.S. patent application Ser. No.11/376,160, filed Mar. 16, 2006, the entire contents of each areincorporated herein by reference.

DISCUSSION OF THE BACKGROUND

1. Field Of The Invention

The invention relates to a three-dimensional (3D) imaging sensor andmethod for controlling an autonomous vehicle.

2. Background of the Invention

In a modern vehicle, the driver remains a critical component of thevehicle's control system as the driver makes numerous decisions directedto the safe operation of the vehicle including speed, steering, obstacleand hazard recognition, and avoidance thereof. Yet, the driver's abilityto perform all of these functions can become compromised due to physicalfactors such as driver fatigue, driver impairment, driver inattention,or other factors such as visibility that reduce the reaction time neededby the driver to successfully avoid hazards.

Furthermore, in environmentally dangerous surroundings such as forexample in warfare settings or in settings where toxic or nuclearradiation hazards are present, the driver is at risk. Indeed, roadsidebombs in Iraq are just one contemporary example of the loss of humanlife which could in many situations be avoided if supply trucks bringingmaterials to the troops were unmanned.

In other more conventional environments, the driver may becomedisoriented or incapable of physically commanding the vehicle as wouldoccur if the driver suffered a medical emergency or if for example thedriver became disoriented under the driving conditions. One example ofsuch a disorienting or incapacitating environment would be a car or shipbeing driven or steered under snow, fog, rain, and/or nighttime blackoutconditions where the diver (or captain of the ship) is handicapped inhis or her ability to perceive and react to hazards approaching or towhich the ship is approaching.

Thus, whether addressing human deficiencies in the control of a vehicleor whether in environmentally hazardous conditions where human controlis not preferred, there exists a need to have a system and method forvehicular identification of objects in the path of the vehicle.

Numerous articles on the development of autonomously driven vehicles andlaser detection and visualization systems have been reported such as thefollowing reference articles all of which are incorporated herein byreference:

-   -   1) H. Wang, J. Kearney, J. Cremer, and P. Willemsen, “Steering        Autonomous Driving Agents Through Intersections in Virtual Urban        Environments,” 2004 International Conference on Modeling,        Simulation, and Visualization Methods, (2004);    -   2) R. Frezza, G. Picci, and S. Soatto, “A Lagrangian Formulation        of Nonholonomic Path Following,” The Confluence of Vision and        Control, (A. S. Morse et al. (eds), Springer Verlag, 1998);    -   3) J. Shirazi, Java Performance Tuning, (OReilly & Associates,        2000);    -   4) J. Witt, C. Crane III, and D. Armstrong, “Autonomous Ground        Vehicle Path Tracking,” Journal of Robotic Systems, (21(8),        2004);    -   5) C. Crane III, D. Armstrong Jr., M. Torrie, and S. Gray,        “Autonomous Ground Vehicle Technologies Applied to the DARPA        Grand Challenge,” International Conference on Control,        Automation, and Systems, (2004);    -   6) T. Berglund, H. Jonsson, and I. Soderkvist, “An        Obstacle-Avoiding Minimum Variation B-spline Problem,”        International Conference on Geometric Modeling and Graphics,        (July, 2003);    -   7) D. Coombs, B. Yoshimi, T. Tsai, and E. Kent, ‘Visualizing        Terrain and Navigation Data,” NISTIR 6720, (Mar. 1, 2001);    -   8) U.S. Pat. No. 5,644,386 to Jenkins et al;    -   9) U.S. Pat. No. 5,870,181 to Andressen;    -   10) U.S. Pat. No. 5,200,606 to Krasutsky et al; and    -   11) U.S. Pat. No. 6,844,924 to Ruff et al;        Despite this work, realization of suitable visualization,        obstacle identification, and obstacle avoidance systems and        methods has not been without problems limiting the operation of        vehicles.

SUMMARY OF THE INVENTION

Accordingly, one object of the present invention accomplished in variousof the embodiments is to provide a system and method for vehicle objectidentification in which a location of the object relative to the vehicleis identified.

Another object of the present invention accomplished in various of theembodiments is to provide a system and method for vehicle objectidentification in which a path for the vehicle relative to the locationof the object is identified.

Yet another object of the present invention accomplished in various ofthe embodiments is to provide a system and method for identifyingobjects in a field of view of the vehicle as obstacles.

Still another object of the present invention accomplished in various ofthe embodiments is to provide steering and vehicle control directions toavoid the obstacles

Various of these and other objects are provided for in certain ones ofthe embodiments of the present invention.

In one embodiment of the present invention, a navigation and controlsystem includes a sensor configured to locate objects in a predeterminedfield of view from a vehicle. The sensor has an emitter configured torepeatedly scan a beam into a two-dimensional sector of a plane definedwith respect to a first predetermined axis of the vehicle, and adetector configured to detect a reflection of the emitted beam from oneof the objects. The sensor includes a panning mechanism configured topan the plane in which the beam is scanned about a second predeterminedaxis to produce a three dimensional field of view. The navigation andcontrol system includes a processor configured to determine theexistence and location of the objects in the three dimensional field ofview based on a position of the vehicle and a time between an emittanceof the beam and a reception of the reflection of the emitted beam fromone of the objects.

In one embodiment of the present invention, a method for navigation andcontrol of a vehicle includes scanning a beam into a sector of a planedefined with respect to a first predetermined axis of the vehicle,detecting a reflection of the emitted beam from an object removed fromthe vehicle, panning the plane in which the beam is scanned about asecond predetermined axis to produce a three dimensional field of view,and determining the existence and location of the object in the threedimensional field of view based on a position of the vehicle and a timebetween an emittance of the beam and a reception of the reflection ofthe emitted beam.

It is to be understood that both the foregoing general description ofthe invention and the following detailed description are exemplary, butare not restrictive of the invention.

BRIEF DESCRIPTION OF THE DRAWINGS

A more complete appreciation of the present invention and many attendantadvantages thereof will be readily obtained as the same becomes betterunderstood by reference to the following detailed description whenconsidered in connection with the accompanying drawings, wherein:

FIG. 1A is a schematic illustration of an autonomous vehicle accordingto one embodiment of the present invention in which a two-dimensional(2D) scan is made in a sector of a plane normal to a predetermined axisof a vehicle;

FIG. 1B is a schematic illustration of an autonomous vehicle accordingto one embodiment of the present invention in which a three-dimensional(3D) scan is made by displacing the scan out the plane normal to thepredetermined axis of a vehicle;

FIG. 2 is a schematic illustration of an emitter and detector systemaccording to one embodiment of the present invention;

FIG. 3 is a detailed schematic illustration showing a perspectivefrontal view of a laser radar (LADAR) imaging sensor according to oneembodiment of the present invention;

FIG. 4 is a detailed schematic illustration showing a perspective rearview of the laser radar (LADAR) imaging sensor depicted in FIG. 3;

FIG. 5 is a contour map of a 3D scan obtained by methods of the presentinvention;

FIG. 6A is a flow chart illustrating particular method steps of thepresent invention;

FIG. 6B is a flow chart illustrating particular method steps of thepresent invention for obstacle identification and avoidance;

FIG. 7A is a schematic illustration of a vehicle according to thepresent invention utilizing multiple LADAR imaging sensors to scanseparate fields of view;

FIG. 7B is a schematic illustration of a vehicle according to thepresent invention utilizing multiple LADAR imaging sensors to scan thesame or overlapping fields of view;

FIG. 8 is a schematic illustration of an exemplary computer system ofthe present invention;

FIG. 9 is a high level schematic of a computing network system of thepresent invention;

FIG. 10 is a high level schematic of the vehicle control system of thepresent invention; and

FIGS. 11-13 are schematics of path determinations made by the presentinvention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Referring now to the drawings, wherein like reference numerals designateidentical, or corresponding parts throughout the several views, and moreparticularly to FIG. 1A, which depicts an imaging sensor 8 mounted, inone embodiment, on top of a vehicle 10 in which a two-dimensional (2D)scan is made in a sector of a plane 11 normal to a predetermined axis ofthe vehicle 10 referred to here for illustration purposes as a“vertical” scanning plane. The imaging sensor 8 includes in oneembodiment an emitter 12 (as shown in FIG. 2) that transmits laserpulses (or light) 14 from the imaging sensor 8 into the environmentabout the vehicle 10. As shown in FIG. 1A, the laser (or light) pulses14 are emitted into the vertical scanning plane 11. To produce athree-dimensional (3D) image, the imaging sensor 8 is panned (oroscillated) in and out of plane 11 to create a 3D scanning volume 16, asshown in FIG. 1B. The imaging sensor 8 detects objects 22 (as shown inFIG. 1B) in the environment nearby the vehicle 10 by detecting lightreflected from the objects 22.

As shown in FIG. 2, the imaging sensor 8 includes a detector 18 fordetecting return of an echoed signal 20. The imaging sensor 8 utilizes aprocessor 24 for controlling the timing and emission of the laser pulses14 and for correlating emission of the laser pulses 14 with reception ofthe echoed signal 20. The processor 24 may be on-board the vehicle or apart of the imaging sensor 8. Details of exemplary processors and theirfunctions are provided later.

In an exemplary example, laser pulses 14 from emitter 12 pass through abeam expander 13 a and a collimator 13 b. The laser pulses 14 arereflected at a stationary mirror 15 a to a rotating mirror 26 and thenforwarded through lens 27 a and a telescope 27 b to form a beam for thelaser pulses 14 with a diameter of 1-10 mm, providing a correspondingresolution for the synthesized three-dimensional field of view. Thetelescope 27 b serves to collect light reflected from objects 22.

In one embodiment of the present invention, the detector 18 isconfigured to detect light only of a wavelength of the emitted light inorder to discriminate the laser light reflected from the object back tothe detector from background light. Accordingly, the imaging sensor 8operates, in one embodiment of the present invention, by sending out alaser pulse 14 that is reflected by an object 22 and measured by thedetector 18 provided the object is within range of the sensitivity ofthe detector 18. The elapsed time between emission and reception of thelaser pulse permits the processor 24 is used to calculate the distancebetween the object 22 and the detector 18. In one embodiment of thepresent invention, the optics (i.e., 13 a, 13 b, 15 a, 26, 27 a, and 27b) are configured to direct the beam instantaneously into the sectorshown in FIG. 1A, and the detector 18 is a field-programmable gate arrayfor reception of the received signals at predetermined angular positionscorresponding to a respective angular direction α₁ shown in FIG. 1A.

Via the rotating mirror 26, laser pulses 14 are swept through a radialsector a within plane 11, as shown illustratively in FIG. 1A. In oneembodiment of the present invention, in order to accomplish mapping ofobjects in the field of view in front of the imaging sensor 8, therotating mirror 26 is rotated across an angular displacement rangingfrom 30 to 90 degrees, at angular speeds ranging from 100-10000 degreesper second. For example, a 90 degree scanning range can be scanned 75times per second or an 80 degree scanning range can be scanned between 5and 100 times per second. Furthermore, the angular resolution can bedynamically adjusted (e.g., providing on command angular resolutions of0.01, 0.5, 0.75, or 1 degrees for different commercially availablesensors 8.

Commercially available components can be used for the emitter 12 and thedetector 18 to provide ranging measurements. In one embodiment, theemitter 12, the detector 18, and the associated optics constitute alaser radar (LADAR) system, but other systems capable of making precisedistance measurements can be used in the present invention, such as forexample a light detection and ranging (LIDAR) sensor, a radar, or acamera. LIDAR (Light Detection and Ranging; or Laser Imaging Detectionand Ranging) is a technology that determines distance to an object orsurface using laser pulses. Like the similar radar technology, whichuses radio waves instead of light, the range to an object is determinedby measuring the time delay between transmission of a pulse anddetection of the reflected signal. LADAR (Laser Detection and Ranging)refers to elastic backscatter LIDAR systems. The term laser radar isalso in use, but with laser radar laser light (and not radio waves) areused.

The primary difference between LIDAR and radar is that with LIDAR, muchshorter wavelengths of the electromagnetic spectrum are used, typicallyin the ultraviolet, visible, or near infrared. In general it is possibleto image a feature or object only about the same size as the wavelength,or larger. Thus, LIDAR provides more accurate mapping than radarsystems. Moreover, an object needs to produce a dielectric discontinuityin order to reflect the transmitted wave. At radar (microwave or radio)frequencies, a metallic object produces a significant reflection.However non-metallic objects, such as rain and rocks produce weakerreflections, and some materials may produce no detectable reflection atall, meaning some objects or features are effectively invisible at radarfrequencies. Lasers provide one solution to these problems. The beamdensities and coherency are excellent. Moreover the wavelengths are muchsmaller than can be achieved with radio systems, and range from about 10micrometers to the UV (e.g., 250 nm). At these wavelengths, a LIDARsystem can offer much higher resolution than radar.

To produce a three-dimensional (3D) image, in one embodiment of thepresent invention, the imaging sensor 8 is panned (or oscillated) in andout the plane 11 to create a 3D scanning volume 16, as shown in FIG. 1B.For sake of illustration, FIG. 1B defines the scanning volume 16 by theangle α (in the vertical scanning direction) and the angle β (in thehorizontal scanning direction). The angle α, as noted earlier, rangesfrom 30 to 70 degrees, at angular speeds ranging from 100-1000 degreesper second. The angle β (i.e., the panning angle) ranges from 1 to 270degrees, at a panning rate ranging from 1-150 degrees per second.Combined the imaging sensor 8 typically can completely scan the 3Dscanning volume 16 at more than two times a second.

In order to accurately determine the distance to objects in the 3Dscanning volume 16, the direction that the imaging sensor 8 is pointedat the time of receiving light reflected from the objects 22 is needed(i.e., the angle of deflection from plane 28 is needed). Further, in oneembodiment of the present invention, geospatial positional data of theinstantaneous vehicle position is utilized by processor 24 to calculatebased on the distance of the object from the vehicle and its directionfrom the vehicle, the geospatial location of the objects in the field ofview. In one configuration of the present invention, the processor 24includes a personal computer running on a Linux operating system, andthe algorithms are programmed in Java programming language. Othercomputing systems and programming languages can be used in the presentinvention (as discussed in more detail below). As shown in FIG. 2,processor 24 is in communication with a real time positioning device 25,such as for example a global positioning system (GPS) and/or an inertialnavigation system (INS), that transmits the location, heading, altitude,and speed of the vehicle multiple times per second to processor 24. Thereal time positioning device 25 is typically mounted to the vehicle 10and transmits data (such as location, heading, altitude, and speed ofthe vehicle) to all imaging sensors 8 (and all processors 24) on thevehicle 10.

With commercially available GPS and the INS units, processor 24 candetermine a position of an object in the field of view to an accuracy ofbetter than 10 cm. In one embodiment of the present invention, theprocessor 24 correlates GPS position, LADAR measurements, and angle ofdeflection data to produce a map of obstacles in a path of the vehicle.The accuracy of the map depends on the accuracy of the data from thepositioning device 25. The following are illustrative examples of theaccuracies of such data: position 10 cm, forward velocity 0.07 km/hr,acceleration 0.01%, roll/pitch 0.03 degrees, heading 0.1 degrees,lateral velocity 0.2%.

In one embodiment of the present invention, a Kalman filter(commercially integrated) sorts through all data inputs to processor 24.A Kalman filter is a known method of estimating the state of a systembased upon recursive measurement of noisy data. In this instance, theKalman filter is able to much more accurately estimate vehicle positionby taking into account the type of noise inherent in each type of sensorand then constructing an optimal estimate of the actual position. Suchfiltering is described by A. Kelly, in “A 3d State Space Formulation ofa Navigation Kalman Filter for Autonomous Vehicles,” CMU RoboticsInstitute, Tech. Rep., 1994, the entire contents of which areincorporated herein by reference. The Kalman filter is a set ofmathematical equations that provides an efficient computational(recursive) means to estimate the state of a process, in a way thatminimizes the mean of the squared error. The filter is very powerful inseveral aspects: it supports estimations of past, present, and evenfuture states, and it can do so even when the precise nature of themodeled system is unknown.

The positioning device 25, by including GPS and INS data, providecomplementary data to the processor 24. GPS and INS have reciprocalerrors. That is GPS is noisy with finite drift, while INS is not noisybut has infinite drift. Further, the processor 24 can be configured toaccept additional inputs (discussed below) to reduce drift in itsestimate of vehicle position when, for example the GPS data may not beavailable.

FIG. 3 is a detailed schematic illustration of imaging sensor 8 of thepresent invention. FIG. 3 presents a frontal view of imaging sensor 8.FIG. 4 presents a rear perspective view of imaging sensor 8. FIG. 3shows a motor 30 configured to oscillate the imaging sensor 8 in and outof a plane normal to a predetermined axis of the imaging sensor. In oneembodiment of the present invention, a 12-volt DC motor operating at aspeed of 120 RPM is used to oscillate the imaging sensor 8 in and outthe plane. Other motors with reciprocating speeds different than 120 RPMcan be used.

As shown in FIG. 3, an absolute rotary encoder 32 is placed on a shaft34 that is oscillating. The encoder 32 provides an accurate reading ofthe angle at which the shaft 34 is instantaneously located. By theencoder 32, an accurate measurement of the direction that the imagingsensor 8 is pointed, at the time of the scan, is known. In oneembodiment of the present invention, the encoder 32 is an ethernetoptical encoder (commercially available from Fraba Posital), placed onshaft 34 to provide both the angular position and angular velocity ofthe shaft.

To decrease the delay between reading a value from the sensor andreading a value from the encoder, a separate 100 MBit ethernetconnection with its own dedicated ethernet card connected the processor24 with the encoder. This created communications delays between theencoder and the I/O computer that were consistent at approximately 0.5ms. Testing revealed that an actual LADAR scan was taken approximately12.5 ms before the data was available at the I/O computer. When thistime was added to the 0.5 ms of delay from the encoder communications, a13 ms delay from the actual scan to the actual reading of the encoderposition and velocity was present. To counteract the angular offset thisdelay created, in one embodiment of the present invention, the velocityof the encoder is multiplied times the communications delay of 0.013seconds to calculate the angular offset due to the delay. This angularoffset (which was either negative or positive depending on the directionof oscillation) was then added to the encoder's position, giving theactual angle at the time when the scan occurred. This processing permitsthe orientation of the LADAR platform to be accurate within 0.05degrees.

Further, as shown in FIGS. 3 and 4, the metal shaft 34 is attached to adetector bracket 36 which is supported by a metal casing 38 with bearing40. Bearing 40 is attached to metal casing 38 with a fastening mechanismsuch as bolts 42 and 44. Detector bracket 36 is attached to metal shaft34. Further, as shown in FIGS. 3 and 4, metal shaft 46 is attached tobearing 48. Bearing 48 is attached to metal casing 38 with a fasteningmechanism such as bolts 50 and 52. Push rod 54 is attached to detectorbracket 36 with ball joint 56 on slot 58. Push rod 54 is attached topivot spacer 60 with ball joint 62. Pivot spacer 60 is attached to servoarm 64 on slot 66. Servo arm 64 is attached to metal shaft 68. Motor 30is attached to servo arm 64 and is suspended from metal casing 38 bymotor mounts 70, 72, and 74.

The imaging sensor 8 operates, in one embodiment, by oscillating ameasurement sensor laterally about an axis of the vehicle 10, as shownin FIG. 1A. In the one embodiment, the shaft 68 of motor 30 rotates at aconstant speed, causing servo arm 64 to also spin at a constant speed.One end of push rod 54 moves with servo arm 64, causing detector bracket36 to oscillate back and forth. The degree of rotation can be adjustedby moving the mount point of ball joint 56 along slot 58, and/or themount point of ball joint 62 along slot 66. Moving the mount pointcloser to shaft 46 increases the angle of rotation, while moving themount point away from shaft 46 decreases the angle of rotation.

While sensor 18 is oscillating, the sensor 18 is taking measurements ofthe surrounding environment along the vertical scanning plane, as shownin FIG. 1A. The absolute rotary encoder 32 operates as an angularposition mechanism, and transmits the absolute angle of deflection ofdetector bracket 36 to processor 24. At the same time, a real timepositioning device 76, such as a global positioning system (GPS) or aninertial navigation system (INS), transmits the location, heading,altitude, and speed of the vehicle multiple times per second toprocessor 24. Software running on the processor 24 integrates the data,and, in one embodiment, uses matrix transformations to transform the YZmeasurements from each 2D scan (as shown in FIG. 1) into a 3D view ofthe surrounding environment. Due to the use of the real time positioningdevice, in the present invention, a terrain map can be calculated evenwhile the vehicle is moving at speeds in excess of 45 miles per hour.

Obstacle Detection Algorithms

The imaging sensor 8 is configured, in one embodiment of the presentinvention, to receive data in the processor regarding echoed or returnedsignals, GPS position, LADAR measurements, and angle of deflection datato determine what sectors of the 3D scan contain obstacles, andtherefore to determine which sectors are most likely impassable by thevehicle. In one embodiment of the present invention, data coming fromdetector 18 is formatted in a coordinate system relative to the detector18, as detailed in the exemplary illustrations below.

Since the imaging sensor 8 is in a “vertical” configuration, thecoordinate system in that configuration has Y-axis representing adistance from the sensor roughly parallel with the forward direction ofthe vehicle, with the X-axis lined up along the vertical direction.Hence, in one embodiment of the present invention, a search forobstacles is manifest in the slope and extent in the X-axis. Tofacilitate this search, all measurement points in the detector's scanare sorted by the Y-values (i.e., by a distance from the detector 18).

FIG. 5 is a schematic representation of a contour map showing thereflected signatures from an object in a synthesized field of view. FIG.5 depicts two common obstacles that the vehicle 10 needs to avoid.Obstacle 22 a is a depression in the highway (e.g., a pothole). Obstacle22 b is an obstruction in the highway (e.g., a fallen boulder). As shownin FIG. 5, lines of constant elevation have been added to the drawing inFIG. 5 for clarity. The imaging sensor 8 of the present invention, asshown in FIG. 1A, produces a vertical scan, which in FIG. 5 is showndepicted on an X-Y axis. A vertical scan provides the advantage ofdirectly supplying slope data (i.e., ΔX/ΔY data) along the road andobstacles shown in FIG. 5 without having to complete a full 3D scan, asshown in FIG. 1B, and then process the image map for slope data.Furthermore, in data from a vertical scan plane as in FIG. 1A, obstaclesappear as either clusters of similar distance or gaps in distance,making obstacle identification more discernable in the data.

According to one embodiment of the present invention, any point P isclassified as part of an obstacle if and only if there could be found apoint Q such that Equations (1), (2), and (3) are satisfied (whereh_(max) is the maximum height or depth, m_(max) is the maximum slope, isthe unit vector in the X direction, and j is the unit vector in the Ydirection).

Q·ĵ>P·ĵ  (1)

$\begin{matrix}{{\frac{{Q \cdot \hat{i}} - {P \cdot \hat{i}}}{{Q \cdot \hat{j}} - {P \cdot \hat{j}}}} > m_{{ma}\; x}} & (2)\end{matrix}$|Q·î−P·î|>h _(max)  (3)

Equation (1) represents a condition in which there exists a first pointQ in the field of view that is farther away from the vehicle than asecond point P. Equation (2) represents a condition in which a slopebetween the first point and the second point is greater than apredetermined slope. Equation (3) represents a condition in which aheight between the first point and the second point is greater than apredetermined height.

Once an obstacle is found relative to the sensor, its location must betranslated into the global reference frame (i.e., a geospatial referenceframe). This can be accomplished in two similar steps. In each step, atransformation matrix is defined so that

P ₂ =T _(1→2) P ₁+Δ₁  (4)

where T_(1→2) is the transformation matrix for going from coordinateframe 1 to coordinate frame 2, Δ₁ is the vector representing theposition of the coordinate frame 1 with respect to coordinate frame 2,and P₁ and P₂ are the same point in coordinate frames 1 and 2,respectively.

The first step converts from the detector coordinate frame to thevehicle's coordinate frame. The transformation matrix, T_(s→v), isdefined as

$\begin{matrix}{T_{s->v} = {{\begin{bmatrix}{\cos \; \psi_{s}} & {{- \sin}\; \psi_{s}} & 0 \\{\sin \; \psi_{s}} & {\cos \; \psi_{s}} & 0 \\0 & 0 & 1\end{bmatrix}\begin{bmatrix}{\cos \; \theta_{s}} & 0 & {\sin \; \theta_{s}} \\0 & 1 & 0 \\{{- \sin}\; \theta_{s}} & 0 & {\cos \; \theta_{s}}\end{bmatrix}}\left\lbrack \begin{matrix}1 & 0 & 0 \\0 & {\cos \; \varphi_{s}} & {{- \sin}\; \varphi_{s}} \\0 & {\sin \; \varphi_{s}} & {\cos \; \varphi_{s}}\end{matrix}\; \right\rbrack}} & (5)\end{matrix}$

where Ψ_(s), θ_(s) and φ_(s) are the yaw (around the z-axis), pitch(around the y-axis), and roll (around the x-axis) of the detectorcoordinate frame relative to the vehicle's coordinate frame. Thistransformation takes into account deviations in yaw, pitch, or rollcaused by the mounting of the sensor. For example, if the detector weremounted pointed slightly downward, it would have a negative pitch thatwould need to be countered by setting θ_(s) to its inverse (or positive)value. In addition, the angle of deflection caused by the oscillation isprocessed here by adding it to θ_(s).

In one embodiment of the present invention, the same basictransformation and translation is performed again in order to translatethe obstacle's location from the vehicle's coordinate system to a globalsystem. Yet another transformation matrix, T_(v→g), is constructed forthis purpose.

$\begin{matrix}{T_{v->g} = {{\begin{bmatrix}{\cos \; \psi_{v}} & {{- \sin}\; \psi_{v}} & 0 \\{\sin \; \psi_{v}} & {\cos \; \psi_{v}} & 0 \\0 & 0 & 1\end{bmatrix}\begin{bmatrix}{\cos \; \theta_{v}} & 0 & {\sin \; \theta_{v}} \\0 & 1 & 0 \\{{- \sin}\; \theta_{v}} & 0 & {\cos \; \theta_{v}}\end{bmatrix}}\begin{bmatrix}1 & 0 & 0 \\0 & {\cos \; \varphi_{v}} & {{- \sin}\; \varphi_{v}} \\0 & {\sin \; \varphi_{v}} & {\cos \; \varphi_{v}}\end{bmatrix}}} & (6)\end{matrix}$

where Ψ_(v), θ_(V) and φ_(v) are the heading (around the z-axis), pitch(around the y-axis), and roll (around the x-axis) of the vehiclerelative to the global coordinate frame. These heading, pitch, and rollvalues are generated by the GPS/INS navigation sensor that is mounted onthe vehicle.

After taking into account both of these transformation matrices, thefull equation for transformation from detector frame to the globalcoordinate frame is:

P _(g) =T _(v→g)(T _(s→v) P _(s)+Δ_(s))+Δ_(v).  (7)

where Δ_(s) is the vector representing the position of the sensor withrespect to the center of the vehicle and Δ_(v) is the vectorrepresenting the position of the center of the vehicle with respect tothe center of the GPS/INS navigation sensor.

At this point, in one embodiment of the present invention, thegeospatial locations of all obstacles in the LADAR sensor's current scanare known. Each of these obstacles is now checked to determine if it canbe safely ignored. If an obstacle is far enough away, it can safely beignored for the time being. FIG. 5 shows for the purposes ofillustration a point R beyond the obstacles 22 a and 22 b. In somesituations, data regarding the positioning of objects (or otherobstacles) beyond this distance can be discarded for example until thevehicle has safely navigated the obstacles 22 a and 22 b. The distanceto the point R will also depend on the speed of the vehicle and theaccuracy of the detector 18 to see objects beyond a sensitivity of thedetector 18. Discarding object data serves two purposes. One, thefurther away an object is from the LADAR unit, the higher the chancethat it is a false signature. False signatures could cause theautonomous vehicle to be unable to find a path, so they must be avoidedat all costs. Two, by reducing the number of obstacles that must beprocessed at any given time, less computing resources are needed. Theremaining obstacles are now processed to determine if the vehicle'scurrent path will collide with any of these obstacles. If any of theseobstacles will cause a collision, the path is shifted to avoid theobstacles (as will be discussed later).

Associated with the distance criteria, the processor 24 in oneembodiment of the present invention discards objects in the field ofview of the detector 18 that are beyond a travel-point distance. Thetravel-point distance (also exemplified by point R on FIG. 5) is in oneembodiment the calculated distance that is an arbitrary factor (e.g.,factors of 5, 10, 100) multiplied times the distance that the vehiclewill travel at the current speed before the next complete 3D scan ofangular sector 16 is made. The travel-point distance is in oneembodiment set to an arbitrary distance (e.g., 100 m). The travel-pointdistance in different embodiments of the present invention can becalculated based on linear, sigmoid, or exponential functions that areevaluated using values such as vehicle speed and time and path. Bydiscarding objects in the field of view beyond the travel-pointdistance, the processor will not be required to process data associatedwith objects so far removed from the vehicle that determination ofwhether or not the objects are obstacles to the path of the vehicle willmake no difference in the steering of the vehicle.

Accordingly, in one embodiment of the present invention, every verticalscan produces from the slope and height data a set of obstacles thatmeet the vector criteria defined in eqns. (1)-(3). Since the verticalscan represents a very small slice of the 3D viewing field, the locationof the obstacle can be represented by a polygon represented in thisillustrative example by a series of spatial coordinate points includinga center point and four corner points: 0.2 m from the back of center,0.2 m from front of center, 0.2 m from left of center, and 0.2 m fromright of center. In one embodiment of the present invention, spatialindices such as a quadtree stores the polygon and efficiently subdividesspace, so that object retrieval (such as identifying if an obstacleexists within a space) is faster than having to search a plot mapcontaining obstacles on a point-by-point analysis. Fitch et al., U.S.Pat. No. 6,212,392, the entire contents of which are incorporated hereinby reference, describe the use of quadrees to determine whether or not alocation associated with a wireless station is within an area ofinterest. Bao et al., “LOD-based Clustering Techniques for EfficientLarge-scale Terrain Storage and Visualization,” in Proceedings SPIEConference on Visualization and Data Analysis, pages 225-235 (2003), theentire contents of which are incorporated herein by reference, describememory algorithms suitable for the present invention in which clusteringalgorithms and data structures including hierarchical quadreetriangulation are explained in detail.

In one embodiment of the present invention, a navigation and controlsystem includes a sensor (e.g., imaging sensor 8) configured to locateobjects in a field of view from a vehicle (e.g., vehicle 10) and aprocessor (e.g., processor 24) configured to determine the existence andlocation of the objects in the field of view based on a position of thevehicle and a time between an emittance of a beam from the sensor and areception of a reflection of the emitted beam from one of the objects.In this embodiment, the processor includes a memory including storageelements corresponding to subsectors in the field of view. The processoris configured 1) to perform an obstacle identification algorithm todetermine if any object is an obstacle, 2) to determine in which of thefield of view subsectors a determined obstacle exists, and 3) to storein each corresponding memory element an obstacle indicator indicativethat an obstacle exists in the corresponding subsector. In oneembodiment of the present invention, processor 24 is configured todetermine if a projected path of the vehicle intersects any subsectorstoring an indicator. In one embodiment of the present invention,processor 24 is configured to execute an obstacle identificationalgorithm including: determining first and second points on a contourmap of an object, determining if the first point is farther away fromthe vehicle than the second point, determining a slope between the firstpoint and the second point, determining if the slope is greater than apredetermined slope, and determining if a height between the first pointand the second point is greater than a predetermined height, and if so,determining the object is an obstacle.

A path planning algorithm (to be discussed in more detail later)according to one embodiment of the present invention presents a “query”polygon for a sector in front of the vehicle and determines from thespatial index (e.g., a quadtree) if any of the stored obstacle polygonsintersect the “query” polygon. If so, a path around the query polygon ischosen. If not, the path is clear of obstacles. The way that a quadtree(or most spatial indices) eliminates sectors of space is by keepingtrack of where it has inserted obstacles. For the quadtree techniquesused in one embodiment of the present invention, the quadtree subdividesspace as obstacles are inserted, thereby creating an indicator that thissector of space contains an obstacle. This leaves many sectors that areempty of obstacles, and the quadtree knows that these sectors are empty.Therefore, if the query is within one of these sectors, it cannot havean obstacle collision.

One conventional way to determine the number of obstacles in the path ofthe vehicle is to mount stationary LADAR units pointed at the ground.With these LADAR units being pointed at the ground, the angle at whichthe LADAR beam hits the ground is calculated. Based on this angle,simple trigonometry can determine where the object is located relativeto the car. In most conventional cases, multiple stationary LADAR unitsare used and each is placed at a different known angle.

There are several disadvantages to the conventional approach:

1) The conventional approach requires movement from the autonomousvehicle in order to map the environment. This means that while thevehicle is stationary, virtually no mapping is happening. The only databeing mapped is from the same scans over and over again. Another problemwith relying on vehicle movement to map data is that the integrity ofthe scans is dependent on the vehicle's speed. As the vehicle goesfaster, the distance between scans increases. This could lead toobstacles being missed.

2) Moving obstacles cannot be detected reliably. This is due to the factthat the LADAR scans are fixed. If an obstacle were to be in between twoLADAR scans and moving at a similar speed and direction as the vehicle(e.g. another vehicle), the obstacle could be missed in the conventionalapproach.

3) In general, the conventional approach uses more computing resourcesthan the present invention. For example, in the conventional approach,since the LADAR units are scanning horizontally, scan data must beplaced into a large terrain elevation map. To detect obstacles, thisterrain elevation map must be searched. This is a computationallyexpensive process.

In one embodiment of the present invention, the imaging sensor 8constantly oscillates to produce the three-dimensional scan.Accordingly, there is no need for vehicle motion to map data. Nor isthere any need in the present invention to store massive amounts of mapor sensor data in order to detect obstacles. Furthermore, the presentinvention in one embodiment only stores data of the geospatial locationsof all the currently-identified obstacles, such as for example anobstacle that is within a certain distance of the vehicle and that hasbeen seen within a certain amount of time. For example, in the presentinvention, an obstacle is considered pertinent if it was within 50meters of the vehicle and if it had been seen within the last 5 seconds.Other definitions of pertinent obstacles based on their distance andhistorical observance in the field of view are possible.

Other advantages of the present invention include the detection ofmoving obstacles and the elimination of blind spot hidden obstacles.Since the imaging sensor 8 in one embodiment of the present inventionrescans the same area multiple times per second, the chances of missingan obstacle are reduced as compared to a single scan over one area. Aslong as the area of oscillation covers a given area, any obstacle withinthat area should be seen.

In one embodiment of the present invention, processor 24 is configuredto perform the illustrative steps shown in FIG. 6A. At step 602, asensing beam is repeatedly scanned into a two-dimensional sector of aplane defined with respect to a first predetermined axis of the vehicle.At step 604, a reflection of the emitted beam from objects removed fromthe vehicle is detected. At step 606, the plane in which the beam isscanned is panned about a second predetermined axis to produce a threedimensional field of view. At step 608, the existence and location ofthe objects in the three dimensional field of view based on a positionof the vehicle and a time between an emittance of the beam and areception of the reflection of the emitted beam is determined.

The method of the present invention in one embodiment emits the beaminto substantially a vertical sector. Accordingly, slope data related tothe terrain and depressions or obstacle protruding from the terrain areaccessible from the scan in the vertical sector without having toprocess the full three dimensional field of view. In one embodiment ofthe present invention, the processor operating with a verticallyoriented sensor 8 does not need to process the full three dimensionalfield of view. The vertical scan lines (e.g., scans in α) originate fromthe vehicle and have slope data encoded in data due to the verticalorientation. Each independent scan line provides data of relevantobstacles.

The method of the present invention in one embodiment obtains a headingof the vehicle and a geospatial position of the vehicle, and processesdata including the distance to the object, the direction to the object,the heading of the vehicle, and the geospatial position of the vehicleto determine whether the path for the vehicle intersects the object forexample based on a predicted position of the vehicle. As discussedbelow, path planning and speed algorithms are used to direct the vehiclearound the obstacles. The method of the present invention in oneembodiment forms a three-dimensional field of view for the vehicle byscanning the beam in an angular sector of the plane while displacing thebeam out of the plane, as shown in FIG. 1B.

The method of the present invention in one embodiment determines anangular coordinate of the beam relative to the second predetermined axisfor the direction to the object and offsets the angular coordinate by anamount indicative of a time delay in receiving a signal indicative ofthe angular coordinate at a processor (used to calculate the location ofthe object). This permits a more accurate determination of the locationof the object.

The method of the present invention in one embodiment directs the beaminto respective angular sectors. The processor can determine thepredetermined angular sectors based on a number of factors including butnot limited to vehicle speed, an identified obstacle location, and aprojected path of the vehicle. In the present invention, the sectorsand/or the panning rate of each sector can be adjusted based upon someof the following non-limiting factors:

1) if an area of interest does not have adequate scan data (i.e. it hasnot been scanned enough times to be confident in that area), the sectorscan be adjusted to give that area of interest more coverage especiallyif the area of interest includes a projected path of an obstacle (i.e.projected path of a moving obstacle) or if the area of interest includesan obstacle with complexity (i.e., if a composite obstacle seems to havemany facets and/or other features warranting more scans), or if the areaof interest includes an obstacle density (i.e., if one area has moreobstacles than another, the sectors and/or speed to cover that area areappropriately adjusted);

2) if other sensor input such as a stereoscopic camera or radar sees ananomaly in an area or if an infrared camera picks up a high heatsignature, for example of a human being, animal, or engine block, thesectors can be adjusted to provide better coverage in that area;

3) if a priori information is known, such as for example from atopographical map indicating that a cliff should be on the left side ofthe road, then the sectors could be adjusted to scan where the cliffshould be to ensure safety;

4) if the vehicle was being operated in a convoy mode (either followinga lead vehicle or possibly a walking human), the sectors could beadjusted to keep the lead vehicle or human in view;

5) if the vehicle was being operated as part of weapons targeting, thesectors could be adjusted to provide higher resolution coverage of thetarget area;

6) if information from other vehicles informs the autonomous vehicleabout an area of interest, then the sectors could be adjusted toconcentrate on the area of interest;

7) if a driver through an input device such a keyboard or mouseindicates an area is of interest, the sectors could be adjusted toconcentrate on the area of interest;

8) if the autonomous vehicle receives a friend or foe signal, thesectors could be adjusted based upon whether the other vehicle isbroadcasting a friend signal or a foe signal; and

9) if the autonomous vehicle expects a landmark to appear in a certainarea, the sectors could be adjusted to look for the landmark in thatarea (landmark recognition is one way to enhance navigational accuracy).

Accordingly, the method of the present invention in one embodimentdetermines the angular sectors (and/or the panning rate to scan with)based on at least one of a vehicle speed, an identified obstaclelocation, a projected path of the vehicle, a resolution required toresolve a complex obstacle or a collection of obstacles to be resolved,other sensory input, an identified priority sector in which an obstaclehas been identified (e.g., a region in which high threat obstaclesexist), and auxiliary information indicating the presence of anobstacle, a moving obstacle, another vehicle, a landmark, or an area (orregion) of interest. When the imaging sensor 8 is configured in avertical orientation, as shown in FIG. 1A, it may not even be necessaryto pan the beam as shown in FIG. 1B, in which case the panning ratecould be set to zero with the obstacles being identified on the basis ofa single scan in which the angle α is varied, as discussed above.

The method of the present invention in one embodiment stores in memoryindicators of whether an obstacle exists in a subsector of the threedimensional field of view. The method of the present invention in oneembodiment further determines if a projected path of the vehicleintersects one of the subsectors containing the indicators.

One method of the present invention for obstacle identification andavoidance is illustrated by the flowchart in FIG. 6B. At 620, thelocation of the object is determined in geospatial coordinates. In thisstep, the method determines angular coordinates (for the direction tothe object) of the beam relative to the predetermined axis, obtainspositioning data from a global positioning system device disposed on thevehicle, and/or obtains orientation data for the predetermined axis ofthe vehicle in a geospatial coordinate system. In this step, the methodprocesses at least one of the angular coordinates, the positioning data,and the orientation data to determine the location of the object ingeospatial coordinates. In one embodiment of the present invention, thedata processed is limited to data for objects within a predetermineddistance of the vehicle.

At 622, the altitude of the vehicle is ascertained, and from thatinformation the present invention determines an elevation of the objectby correlating the location of the object relative to the vehicle withthe geospatial position of the vehicle and the altitude of the vehicle,determines elevations for plural points on the object, and/or generatesa contour map for the object.

At 624, the present invention determines if the object is an obstacle.For instance, as detailed by the vector equations (1)-(3) listed above,one embodiment of the present invention determines if the object is anobstacle by assessing if there exists a first point on a contour map (orin the field of view) that is farther away from the vehicle than asecond point P on the contour map, if a slope between the first pointand the second point is greater than a predetermined slope, and if aheight between the first point and the second point is greater than apredetermined height.

At 626, if the object is an obstacle, the present invention determinesif the obstacle is to be avoided, for example by determining if ageospatial position of the obstacle remains in the path of the vehicleover a time lapse period and/or by determining if a geospatial positionof the obstacle is within a set distance from the vehicle. In oneembodiment of the present invention, the obstacle (now identified) ismapped relative to the path of the vehicle.

At 628, if the object is an obstacle, steering and speed controldirections are provided to the vehicle in order to avoid the obstacle.The steering and speed control directions can take into considerationthe destination of the vehicle, and thereby provide steering and speedcontrol directions to the vehicle in order to avoid the obstacle and toreturn on course to the destination.

Or, at 630, if no obstacles are present, then the steering and speedcontrol directions provide steering and speed control directions to thevehicle to guide the vehicle to the destination.

FIG. 7A is a schematic illustration of a vehicle according to thepresent invention depicting one embodiment in which multiple LADARimaging sensors are used. In this embodiment, one or more of the imagingsensors is dedicated to scanning for the detection of objects nearby thevehicle (e.g., within 50 m) while another of the imaging sensors isdedicated to scanning for the detection of objects farther away from thevehicle (e.g., beyond 50 m).

In another embodiment of the invention, multiple imaging sensors areused for redundancy and to provide different perspectives of the sameobject. FIG. 7B is a schematic illustration of a vehicle according tothe present invention depicting one embodiment in which multiple LADARimaging sensors are used to scan the same or overlapping fields of view.This configuration provides redundant coverage in the center of the pathso that, if one imaging sensor 8 fails, the other one can still senseobstacles most likely to be directly in the vehicle's path. The datafrom the imaging sensors 8 are correlated by placing all data onto thesame elevation grid (as illustrated for example in FIG. 5).

In another embodiment, the imaging sensors 8 are configured to locateobjects removed from a vehicle 10; and processor 24 is configured todirect one of the sensors to scan a first sector associated with a pathof the vehicle, while directing another of the sensors to scan a secondsector identified with an obstacle. As such, the first and/or secondsector determinations can be based on a number of factors including, butnot limited to a vehicle speed, an identified obstacle location, aprojected path of the vehicle, a resolution required to resolve acomplex obstacle or a collection of obstacles to be resolved, sensoryinput other than from the sensors, an identified priority sector inwhich an obstacle has been identified, and auxiliary informationindicating the presence of an obstacle, a moving obstacle, anothervehicle, a landmark, or an area of interest.

In one variant of this embodiment, the processor 24 can direct onesensor to scan a first sector associated with a path of the vehicle, andin a programmed manner direct the same sensor (e.g., in a dynamicfashion) to scan a second sector identified with an obstacle. Factorswhich determine the programmed duty cycle by which one sensor scans thefirst sector and then a second sector include for example the speed ofthe vehicle, the proximity of the obstacle, any movement of theobstacle, an identified status of the obstacle (e.g., friend or foe),the proximity of the obstacle to the projected path of the vehicle, andthe calculated clearance from the vehicle to the obstacle.

Moreover, in one embodiment of the present invention, one of the imagingsensors 8 is dedicated to scanning in a horizontal direction whileanother imaging sensor is directed to scan in the vertical direction.Scan information from this unit permits the processor 24 to betteridentify the general terrain and terrain curvature from which obstaclessuch as shown in FIG. 5 can be identified. Complementary data from bothhorizontal and vertical scans helps identity the edges of compositeobstacles (groups of individual obstacles that should be treated as oneobstacle) more accurately. One of the issues with handling movingobstacles is determining the full proportions of an obstacle. Tocalculate the full proportions of an obstacle, multiple “independent”obstacles are intelligently grouped to form one larger compositeobstacle when for example the data points representing the independentobstacles are within a set distance of each other (e.g., within 100 cm).Moreover, in other embodiments of the present invention, the groupinginto composite obstacles is set by more than just a distance ofseparation between points normally qualifying as an obstacle point.Other factors that can be used in the determination include for examplethe number of times each point identified as an obstacle is seen,whether the obstacle point moves spatially in time, and whether (asdiscussed elsewhere) if there is confirmation of the obstacle by otherimage sensors or stereographic cameras.

Having two completely different perspectives of the obstaclesfacilitates this task by the obstacles being viewed from two separatedimensions (i.e., from top to bottom and from left to right). Since thebeams tend to wrap around the curvature of an obstacle, this providesaccurate estimations of the size and orientation of a compositeobstacle. For instance, consider a spherical boulder. While the backsideof the spherical boulder can not be seen, the sensing beam maps out acontour of the spherical boulder providing the aforementioned size andorientation, providing an estimate of the full size of the sphericalboulder.

Furthermore, having stepper motors (or in general any motor that canhave its position dynamically adjusted) oscillate the platform ratherthan a continuously revolving motor creates several distinctadvantages: 1) the rate at which the platform oscillates can be changeddynamically, and 2) the size of the sector which is scanned can bedynamically changed.

By having these dynamic capabilities, the present invention achievesseveral advantages:

1) Path (or road) curvature can be tracked in the present invention. Asthe path in front of the vehicle curves, the imaging sensors 8 canchange their sector to better match the path, thereby ensuring that thepath always gets the best coverage. This also helps to ensure that hard90 degree or more turns do not create blind spots on the path. Animaging sensor that cannot alter its sector would be fixated ahead ofthe vehicle, even though the path is turning towards the right or left,and therefore blindspots in the viewing field would develop that wouldnot resolved until after the vehicle had started turning. Accordingly,in one embodiment of the present invention, the sensors 8 canpreemptively adjust their orientation. For example, the imaging sensor 8shown in FIG. 1B or the multiple imaging sensors 8 shown in FIGS. 7A and7B can be directed into a three-dimensional field of view that followsthe anticipated path of the vehicle 10 on for example a highway.Further, the angular sectors mapped by the multiple imaging sensors 8can be dynamically adjusted using stepper motors to vary (in real time)the angular sector β scanned and the angular scan rate. Factors whichinfluence selection of the angular sector to be scanned include vehiclespeed, an identified obstacle location relative to a turn in a path ofthe vehicle, a projected path of the vehicle, a resolution required toresolve a complex obstacle or a collection of obstacles to be resolved,sensory input other than from the sensors, an identified priority sectorin which an obstacle has been identified, and auxiliary informationindicating the presence of an obstacle, a moving obstacle, anothervehicle, a landmark, or an area of interest.

2) The speed of oscillation of the platform can be adjusted based on thespeed of the vehicle. If the vehicle is going slower, the speed ofoscillation can be reduced, thereby causing a higher resolution 3D imageof the environment. As the vehicle speeds up, the speed of oscillationcan increase to give better overall coverage of obstacles further downthe road. Accordingly, in one embodiment of the present invention, theangular scan rates for α and β (as shown in FIGS. 1A and 1B can beproportionate to the vehicle speed. Furthermore, in one embodiment ofthe present invention, as the vehicle moves faster, the scanned sectorscould move farther out, or the speed of oscillation could change, or theangular sector could narrow (or all three could occur). The conversewould apply when the vehicle slows down.

3) Obstacles can be tracked in the present invention. Multiple imagingsensors can track obstacles and adjust their sectors to ensure that highthreat obstacles are tracked (i.e., priority sectors or regions ofinterest can be established). For example, in one embodiment of thepresent invention, there are three imaging sensors on the front of thevehicle. If an obstacle is detected, two imaging sensors can keepanalyzing the path, while the other sensor changes its sector to ensurethat the obstacle can be scanned. Such capability permits the trackingof moving obstacles or other vehicles or for passing another vehicle atrelatively high speed. Accordingly, in one embodiment of the presentinvention, respective ones of the imaging sensors 8 can be dedicated toimaging and obstacle identification and tracking in predeterminedsectors.

4) Zone coverage is another advantage that can be realized in thepresent invention. The processor 24 could specify several zones aroundthe vehicle with different levels of priority and multiple imagingsensors could dynamically adjust their sectors to make sure that allzones were covered based upon priority. For example, if the pathdirectly in front of the vehicle were the highest priority and the sidesof the vehicle were lower priority, then the sensors could bedynamically adjusted to always ensure that the front of the vehicle wasadequately covered, and if additional sensors were available, then thesides would be covered too. This approach allows for easy scalabilityand fault tolerance. The processor 24 in one embodiment of the presentinvention could realize that one of the sensors had failed, and thendynamically adjust for the failed sensor to ensure that the proper zoneswere always covered. Adding sensors increases coverage areas andredundancy or fault tolerance. Accordingly, in one embodiment of thepresent invention, respective ones of the imaging sensors 8 providehardware redundancy and field of view redundancy.

Besides input from redundant imaging sensors, the processor 24 in oneembodiment of the present invention receives input from on-board vehiclesensors. One of these inputs is a wheel speed sensor which providestransistor-transistor logic (TTL) pulses based upon an encoder placed ona single wheel on the vehicle. When a wheel speed sensor is added, theprocessor 24 uses GPS data from the positioning device 25 to initiallycalibrate the TTL pulses to actual vehicle movement. In the event thatGPS data is not available, due to for example tunnels, canyons, or otherobstructions, the processor 24 is able to minimize the positional driftby making use of the wheel speed sensor and its latest reliably knowncorrespondence to the vehicle's movement.

The wheel speed sensor included a digital sensor capable of detectingeither ferrous metal or magnets that are in motion. The digital sensorwas mounted in the wheel well adjacent to the stock Antilock BrakeSystem (ABS) sensor, which allowed the wheel speed sensor to read thesame magnets mounted on the wheel that the ABS sensor did. This level ofaccuracy permits the vehicle 10 to precisely know its location on theearth.

Another input to processor 24 (as described later) is input from abiometric sensor or input from command button in which a driver to thevehicle can relinquish or take control of the vehicle 10.

FIG. 8 illustrates one embodiment of a computer system 1201 in which theprocessor 24 (or any of the specific processors discussed below) of thepresent invention can be implemented. The computer system 1201 isprogrammed and/or configured to perform any or all of the functionsdescribed above. Further, respective functions can be divided amongdifferent computers on board the vehicle (as shown for example in thecomputing network system of FIG. 9). These computers may be incommunication with each other via the communications network 1216(discussed below). The computer system 1201 includes a bus 1202 or othercommunication mechanism for communicating information, and a internalprocessor 1203 coupled with the bus 1202 for processing the information.The computer system 1201 includes a memory 1204, such as a random accessmemory (RAM) or other dynamic storage device (e.g., dynamic RAM (DRAM),static RAM (SRAM), and synchronous DRAM (SDRAM)), coupled to the bus1202 for storing information and instructions to be executed by theinternal processor 1203. In addition, the memory 1204 may be used forstoring temporary variables or other intermediate information during theexecution of instructions by the internal processor 1203. The computersystem 1201 preferably includes a non-volatile memory such as forexample a read only memory (ROM) 1205 or other static storage device(e.g., programmable ROM (PROM), erasable PROM (EPROM), and electricallyerasable PROM (EEPROM)) coupled to the bus 1202 for storing staticinformation and instructions for the internal processor 1203.

The computer system 1201 may also include special purpose logic devices(e.g., application specific integrated circuits (ASICs)) or configurablelogic devices (e.g., simple programmable logic devices (SPLDs), complexprogrammable logic devices (CPLDs), and field programmable gate arrays(FPGAs)). The computer system may also include one or more digitalsignal processors (DSPs) such as the TMS320 series of chips from TexasInstruments, the DSP56000, DSP56100, DSP56300, DSP56600, and DSP96000series of chips from Motorola, the DSP1600 and DSP3200 series fromLucent Technologies or the ADSP2100 and ADSP21000 series from AnalogDevices. Other processors especially designed to process analog signalsthat have been converted to the digital domain may also be used (asdetailed in the working example below).

The computer system 1201 performs a portion or all of the processingsteps of the invention in response to the internal processor 1203executing one or more sequences of one or more instructions contained ina memory, such as the main memory 1204. Such instructions may be readinto the main memory 1204 from another computer readable medium, such asa hard disk 1207 or a removable media drive 1208. Such instructions maybe read into the main memory 1204 from another computer readable medium,such as a USB flash drives or jump drives. Such drives are solid-statememory devices which have the ability to act as floppy disks or harddrives under most computer operating systems. One or more processors ina multi-processing arrangement may also be employed to execute thesequences of instructions contained in main memory 1204. In alternativeembodiments, hard-wired circuitry may be used in place of or incombination with software instructions. Thus, embodiments are notlimited to any specific combination of hardware circuitry and software.

As stated above, the computer system 1201 includes at least one computerreadable medium or memory for holding instructions programmed accordingto the teachings of the invention and for containing data structures,tables, records, or other data described herein. Examples of computerreadable media suitable for the present invention are compact discs,hard disks, floppy disks, tape, magneto-optical disks, PROMs (EPROM,EEPROM, flash EPROM), DRAM, SRAM, SDRAM, or any other magnetic medium,compact discs (e.g., CD-ROM), or any other optical medium, punch cards,paper tape, or other physical medium with patterns of holes, a carrierwave (described below), or any other medium from which a computer canread.

Stored on any one or on a combination of computer readable media, thepresent invention includes software for controlling the computer system1201, for driving a device or devices for implementing the invention,and for enabling the computer system 1201 to interact with a human user(e.g., a driver). Such software may include, but is not limited to,device drivers, operating systems, development tools, and applicationssoftware. Such computer readable media further includes the computerprogram product of the present invention for performing all or a portion(if processing is distributed) of the processing performed inimplementing the present invention. The computer code devices of thepresent invention may be any interpretable or executable code mechanism,including but not limited to scripts, interpretable programs, dynamiclink libraries (DLLs), Java classes, and complete executable programs.Moreover, parts of the processing of the present invention may bedistributed for better performance, reliability, and/or cost.

The term “computer readable medium” as used herein refers to any mediumthat participates in providing instructions to the internal processor1203 for execution. A computer readable medium may take many forms,including but not limited to, non-volatile media, volatile media, andtransmission media. Non-volatile media includes, for example, optical,magnetic disks, and magneto-optical disks, such as the hard disk 1207 orthe removable media drive 1208. Volatile media includes dynamic memory,such as the main memory 1204. Transmission media includes coaxialcables, copper wire and fiber optics, including the wires that make upthe bus 1202. Transmission media also may also take the form of acousticor light waves, such as those generated during radio wave and infrareddata communications.

Various forms of computer readable media may be involved in carrying outone or more sequences of one or more instructions to internal processor1203 for execution. For example, the instructions may initially becarried on a disk to a remote computer. An infrared detector coupled tothe bus 1202 can receive the data carried in the infrared signal andplace the data on the bus 1202. The bus 1202 carries the data to themain memory 1204, from which the internal processor 1203 retrieves andexecutes the instructions. The instructions received by the main memory1204 may optionally be stored on storage device 1207 or 1208 eitherbefore or after execution by the internal processor 1203.

For instance, in one embodiment of the present invention, a computerreadable medium contains program instructions for execution on aprocessor in a vehicle, which when executed by the processor, cause theprocessor to scan repeatedly a beam into a two-dimensional sector of aplane defined with respect to a first predetermined axis of the vehicle,to detect a reflection of the emitted beam from an object removed fromthe vehicle, to pan the plane in which the beam is scanned about asecond predetermined axis to produce a three dimensional field of view,and to determine the existence and location of the object in the threedimensional field of view based on a position of the vehicle and a timebetween an emittance of the beam and a reception of the reflection ofthe emitted beam.

In one embodiment of the present invention, a computer readable mediumcontains program instructions for execution on a processor in a vehicle,which when executed by the processor, cause the processor to scanobjects in a field of view from the vehicle, to determine a location ofthe objects in the field of view, to determine first and second pointson a contour map of an identified object, to determine if the firstpoint is farther away from the vehicle than the second point, todetermine a slope between the first point and the second point, todetermine if the slope is greater than a predetermined slope, and todetermine if a height between the first point and the second point isgreater than a predetermined height, and if so, to determine that theobject is an obstacle.

In one embodiment of the present invention, a computer readable mediumcontains program instructions for execution on a processor in a vehicle,which when executed by the processor, cause the processor to scanobjects in a field of view from the vehicle, to determine a location ofthe objects in the field of view, to perform an obstacle identificationalgorithm to determine if any object is an obstacle, to determine afield of view subsector in which a determined obstacle exists, and tostore in a corresponding memory element an obstacle indicator indicativethat an obstacle exists in a corresponding subsector.

In one embodiment of the present invention, a computer readable mediumcontains program instructions for execution on a processor in a vehicle,which when executed by the processor, cause the processor to scanobjects in a field of view from the vehicle, to determine a location ofthe objects in the field of view, to direct a scan in a first sectorassociated with a path of the vehicle, and to direct a scan in a secondsector identified with an obstacle.

In one embodiment of the present invention, a computer readable mediumcontains program instructions for execution on a processor in a vehicle,which when executed by the processor, cause the processor to scanobjects in a field of view from the vehicle, to determine a location ofthe objects in the field of view, to direct a scan selectively in asector associated with a projected turn in a path of the vehicle.

The program instructions, in various embodiments of the presentinvention, are configured to cause the processor to determine which ofthe sectors to scan based on at least one of a vehicle speed, anidentified obstacle location, a projected path of the vehicle, aresolution required to resolve a complex obstacle or a collection ofobstacles to be resolved, sensory input other than from the sensors, anidentified priority sector in which an obstacle has been identified, andauxiliary information indicating the presence of an obstacle, a movingobstacle, another vehicle, a landmark, or an area of interest.

Further, the computer readable medium of the present invention caninclude program instructions detailing geographical informationassociated with a particular locale, path planning algorithms (asdescribed below), navigational instructions, instructions particular toan installed image sensor on the vehicle, instructions for commandand/or receipt of data from additional sensors such a stereoscopiccameras, or vehicle wheel speed sensors, or receipt of data from driverinput control devices or other on-board devices (such as those describedlater), path planning algorithms, a particularized vehicle transferfunction containing data regarding vehicle thrust and response toexternal forces for the autonomous vehicle in use, and steering controlfor the autonomous vehicle in use.

The computer system 1201 also includes a communication interface 1213coupled to the bus 1202. The communication interface 1213 provides atwo-way data communication coupling to a network link 1214 that isconnected at least temporarily to, for example, a local area network(LAN) 1215, or to another communications network 1216 such as theInternet during downloading of software to the processor 24 or aninternal network between multiple computers on board the vehicle. Forexample, the communication interface 1213 may be a network interfacecard to attach to any packet switched LAN. As another example, thecommunication interface 1213 may be an asymmetrical digital subscriberline (ADSL) card, an integrated services digital network (ISDN) card ora modem to provide a data communication connection to a correspondingtype of communications line. Wireless links may also be implemented aspart of the communication interface 1213 to provide data exchange withany of the on-board computers, image sensors, wheel speed sensors,biometric sensors, and/or driver command input devices. In any suchimplementation, the communication interface 1213 sends and receiveselectrical, electromagnetic or optical signals that carry digital datastreams representing various types of information.

The network link 1214 typically provides data communication through oneor more networks to other data devices to provide data exchange with anyof the on-board computers, image sensors, wheel speed sensors, biometricsensors, and/or driver command input devices. For example, the networklink 1214 may provide a temporary connection to another computer througha local network 1215 (e.g., a LAN) or through equipment operated by aservice provider, which provides communication services through acommunications network 1216. As shown in FIG. 8, the computing system1201 can be in communication with an input device 1217 via the localnetwork 1215 and the communications network 1216 which use, for example,electrical, electromagnetic, or optical signals that carry digital datastreams, and the associated physical layer (e.g., CAT 5 cable, coaxialcable, optical fiber, etc). The signals through the various networks andthe signals on the network link 1214 and through the communicationinterface 1213, which carry the digital data to and from the computersystem 1201 may be implemented in baseband signals, or carrier wavebased signals. The baseband signals convey the digital data asunmodulated electrical pulses that are descriptive of a stream ofdigital data bits, where the term “bits” is to be construed broadly tomean symbol, where each symbol conveys at least one or more informationbits. The digital data may also be used to modulate a carrier wave, suchas with amplitude, phase and/or frequency shift keyed signals that arepropagated over a conductive media, or transmitted as electromagneticwaves through a propagation medium. Thus, the digital data may be sentas unmodulated baseband data through a “wired” communication channeland/or sent within a predetermined frequency band, different thanbaseband, by modulating a carrier wave. The computer system 1201 cantransmit and receive data, including program code, through thenetwork(s) 1215 and 1216, the network link 1214, and the communicationinterface 1213. Moreover, the network link 1214 may provide a connectionthrough a LAN 1215 to the various GPS and INS systems on board thevehicle. The input device 1217 in various embodiments of the presentinvention provides input to the processor 24 and representsschematically the image sensors, wheel speed sensors, biometric sensors,and/or driver command input devices discussed in the present invention.

WORKING EXAMPLE

A 2005 Ford Escape Hybrid™ (hereinafter referred to as the workingvehicle) was modified to include the imaging sensors 8 of the presentinvention. The working vehicle used a hybrid drive system in which anelectric engine operates virtually all of the time and in which the gasengine starts and stops automatically to either provide extra horsepoweror to recharge the electric engine's battery. The working vehicle'selectrical system, which was powered by a 330-volt battery, providesover 1300 watts of power to the equipment mounted in the workingvehicle.

The working vehicle utilized a commercially available AdvancedElectronic Vehicle Interface Technology (AEVIT) “drive-by-wire” systemfrom Electronic Mobility Controls (EMC) to physically control the car.The AEVIT system uses redundant servos and motors to turn the steeringwheel, switch gears, apply throttle, and apply brake. This commercialsystem has a proven safety record in the automobile industry due to itsuse of redundant hardware.

The working vehicle provided 110 amps of power at 12 volts. The imagingsensor 8 utilized in the working vehicle operated on 24 volts. Ratherthan provide this power from the working vehicle's 12 volt electricalsystem, an auxiliary 24 volt electrical system for the imaging sensorwas provided. The auxiliary electrical system included two 12 voltbatteries connected in series to provide 24 volts of power. Solar panelswere installed on top of the vehicle to charge the batteries in theauxiliary system.

In the working vehicle, a 2.8 Gigahertz Intel Pentium III™ processorincluding 2 gigabytes of RAM was used as the processor 24. The 2.8Gigahertz Intel Pentium III™ processor operated on 12 volt power supply,so an AC to DC power inverter was not needed. The 2.8 Gigahertz IntelPentium III™ processor hosted the principle computing functions, such asfor example the sensor communication, vehicle controls, and artificialintelligence.

Additionally, the working vehicle included several 1.42 Gigahertz AppleMac Mini™ computers to host the path-planning software. The 1.42Gigahertz Apple Mac Mini™ computers (running on running Apple OS X 10.4software) were programmed to perform all of the path calculations in aredundant cluster to ensure that the path planning software was notsuspect to a single point of failure. The Mac Mini™ computers had lowpower consumption (less than 2.3 amps at peak) and vector mathprocessing capabilities. The processors listed above can be shockmounted on the vehicle to prevent damage from sudden changes in thedirection of the vehicle or the crossing of obstacles along the path.

FIG. 9 shows a high level schematic of a computing network system of thepresent invention employing the computers and sensor data discussedabove. In this networked system, the individual processors have similarcapabilities and computer elements as described with respect to FIG. 8.FIG. 9 shows that on-board electrical motor control (EMC) equipment isdriven by instructions from the Navigational Computer. The NavigationalComputer is connected by a switch (shown as a CISCO Switch) to a SensorProcessing Computer and a Path Planning Computer. These computersreceive input from the RT3000™ GPS and INS positioning device (describedbelow). The Path Planning Computer and the Navigational Computer utilizevarious path planning and speed control algorithms (described below) toprovide steering instructions to the vehicle. The Sensor ProcessingComputer as shown in one illustrative embodiment of the presentinvention receives input from imaging sensor 8 (for example the LADARdevices shown) and encoders on the shaft of a LADAR device (for examplethe optical encoders shown). The LADAR devices provide distance data andthe instantaneous angle α of the LADAR device in the predeterminedangular scanning sector as shown in FIG. 1A. In one embodiment, opticalencoders are used as the rotary encoders 32 discussed above and providethe Sensor Processing Computer with the panning angle β relative to forexample a vertical axis, as shown in FIG. 1B. Optical encoders are usedin this embodiment of the present invention because of their highaccuracy. Since optical encoders do not rely on physical contact withthe shaft 34, but rather visually determine movement, optical encodersare more accurate and less prone to failure. The encoders from FrabaPosital were specifically used in the working vehicle because of theirEthernet capability.

The working vehicle utilized RT3000™ positioning devices from OxfordTechnical Solutions to provide vehicle localization (i.e., positioningdata). The RT3000™ uses Omnistar™ HP differential GPS signals to provideposition accuracy of 10 centimeters or less. Integrated INS in theRT3000™ permits the RT3000™ to survive GPS outages of up to 30 secondswith virtually no performance degradation. Because the GPS and INS wereintegrated together, each can compensate for problems with the other.For example, if the INS started to drift laterally, the integrated GPSwill automatically correct that drift. In the working vehicle, twoOxford RT3000™ GPS units were employed in a primary/secondary role.

The working vehicle utilized commercially available laser modulationsystems (LMS) for the imaging sensors 8 such as three SICK™ LMS 291LADAR units and one RIEGL™ LMS-Q 120 LADAR on the front of the vehicle.The LADAR units in the working vehicle were split into three differentroles. The RIEGL™ LADAR was mounted vertically in the center of thevehicle and is used for long range (up to 80 meters) obstacle detection.The two SICK™ LMS 291 units were mounted vertically on either side ofthe RIEGL™. These vertically mounted units were used for short rangeobstacle detection (up to 50 meters). The other SICK™ LMS 291 wasmounted horizontally on the center of the vehicle and is used foranalyzing terrain features.

In order to protect these sensors from the elements as much as possible,a sun/weather guard was constructed over the sensor mounts. This guardprotects the LADAR units from being blinded by direct sun glare, acommon problem with Sick LADAR units. The guard can constitute a simpleoptical shield such as for example a piece of sheet-metal that wasattached to the top of the imaging sensor. The guard extended over thefront of LADAR unit (or other imaging sensor) and had a slight bend tobetter protect the sensors from the sun. Since LADAR units produce theirown light source, low light conditions have no effect on the sensors. Infact, testing showed that low light conditions actually produced betterresults than bright conditions, due to excessive reflections underbright sunlight. These reflections, along with dust or rain, can produceghosting patterns or other anomalies in the sensor readings. Tocounteract these anomalies, the present invention utilizes filteringalgorithms to correct for anomalies before the anomalies can causeproblems in the path planning software. For example, the filteringalgorithm in one embodiment associates isolated obstacle points withnoise data and discards these data points, under the assumption that“real” obstacles have a physical size greater than the resolution of theimaging sensor.

On the working vehicle, stereoscopic cameras capable of producing athree-dimensional point cloud were mounted on the front of the vehiclebehind the windshield. These cameras operated in a secondary mode to theLADAR units. The cameras are used to help identify the road and toconfirm the existence of obstacles. If a LADAR unit identifies anobstacle and the stereoscopic cameras also detect an obstacle in thatarea, the obstacle avoidance algorithms increase the confidence levelassociated with that obstacle. Confidence levels were used to helpdecide which obstacles must absolutely be avoided and which obstaclesare possibly driven over without damage.

FIG. 10 is a high level schematic of the vehicle control system of thepresent invention. FIG. 10 shows an iterative loop control system usinginputs from the sensors and providing vehicle control. For the sake ofillustration, at 102, LADAR data stored in a memory unit for example ofthe Sensor Processing Computer in FIG. 9 is provided to obstacledetection unit 104, speed planning unit 106, and accelerator/brakecontrol 108. GPS/INS unit 110 also provided to the obstacle detectionunit 104. Once the obstacle detection unit 104 decides which objects inthe field of view are obstacles (for example using the conditionsspecified in eqns. (1), (2), and (3) above, obstacle identification datais forwarded to the speed planning unit 106 and to path planning unit112. The path planning unit 112 receives input from a route definitiondata file (RDDF) unit 114 and provides the path data to steering controlunit 116. EMC Vehicle Control 118 takes input from the steering controlunit 116 and the accelerator/brake control 108 to control the workingvehicle.

The obstacle detection unit 104 utilizes in one embodiment of thepresent invention spatial indices to correlate the sensor data withlocalization data indicating in which sector an obstacle is located (forexample a near sector requiring immediate consideration or a far sectornot requiring immediate consideration). FIG. 5 shows a division of thefield of view into sectors: the sector defined between rays A and B, thesector defined between rays B and C, and the sector defined beyond C.Instead of storing all obstacles in one large spatial index, eachsegment of the path corridor has a corresponding spatial index inmemory. Such division in one embodiment permits the present invention tocall up from memory the sectors of importance for example whennavigating obstacles 22 a and 22 b.

The obstacle detection unit 104 in one embodiment of the presentinvention uses times-stamps taken with each LADAR scan and GPS reading.These timestamps are then correlated based upon position and heading toproduce a geospatial location for obstacles, for example the obstaclesthemselves may be moving. Timestamping can counteract anomalies in theimages. Highly reflective surfaces can cause the LADAR devices toregister incorrect distances for obstacles. To counteract theseanomalies, in one embodiment of the present invention, scans are onlykept in memory for a designated amount of time. After that time haspassed, if no more obstacles have been registered in that same area, theobstacle is removed from memory storage. This technique ensures thatmoving obstacles are handled correctly. If another vehicle or anobstacle(s) crosses the vehicle path in a perpendicular direction to theautonomous vehicle, the sensors would effectively register a sequence ofobstacles in front of the autonomous vehicle. These obstacles wouldappear as a complete obstruction of the vehicle's path, forcing thevehicle to immediately stop. After enough time had passed, the obstacleswould expire, and the autonomous vehicle would be able to start movingagain. A persistent obstacle, on the other hand, would not expire.

Consider, for example, a large boulder that is located in the center ofthe path. At a distance of approximately forty to fifty meters, theLADAR devices will start to register parts of the boulder. As vehicle 10gets to within ten to twenty meters of the vehicle, the previous scanswould begin to approach the obstacle expiration time. Since the sensorsare still registering the boulder, the previous scans will not expire.Instead, the previous scans along with the current scans would all beretained, giving the boulder a higher count for total number of scans.This high count for the number of scans causes the boulder to have anextremely high probability of being an obstacle.

The path-planning algorithms of the present invention avoid obstaclesand routinely steer the vehicle within a planned corridor. In oneembodiment, these algorithms without cause will not deviate the vehiclefrom the planned corridor. However, should the vehicle leave the routecorridor for some reason, the navigation system will detect this andprovide a safe route back into the corridor. If a waypoint is missed,the navigation system will simply continue to the next feasible waypointon the path. If the path is obstructed by an obstacle, the path planningsystems will determine a path around the obstacle.

Path planning in one embodiment of the present invention is accomplishedthrough the use of cubic b-splines designed to follow the center of theplanned route while still ensuring that the path is not impossible forthe vehicle to navigate. This assurance means that the curvature at anypoint along the path is below the maximum curvature that the vehicle cansuccessfully follow. In addition, the curvature is kept continuous sothat it is not necessary to stop the vehicle in order to turn thesteering wheel to a new position before continuing. B-splines werechosen for use in the path planning algorithms primarily because of theease in which the shape of their resulting curves can be controlled.After an initial path is created that follows the center of thecorridor, the path is checked against the obstacle repository todetermine if it is a safe path. If the path is not safe, a simplealgorithm generates and adjusts control points on the problem spots ofthe curve until the spline avoids all known obstacles while stillcontaining valid maximum curvature. At this point, the path is both safeand drivable.

The path planning of the present invention can also uses a Level ofDetail (LOD) based obstacle avoidance algorithm along with severalplanning algorithms to plan paths around obstacles. LOD analysis in oneembodiment of the present invention permits running the same algorithmwith different levels of detail. For example, running with less detail(e.g., to accommodate large safety margins), then iteratively increasingthe detail (e.g., to accommodate smaller safety margins) until a validpath is found. The path planning algorithms run using several differentparameters until a valid path is found. The initial parameters usesafety margins (for example the clearance of the vehicle from anobstacle or between obstacles), while the final parameters use no safetymargins around obstacles. This ensures that if a path is available thatwill avoid an obstacle with a large margin of error (e.g., vehiclelateral clearance) the path planning software selects that path.Otherwise, the planning algorithm will keep reducing the safety marginaround obstacles until a valid path is determined.

FIGS. 11, 12, and 13 illustrate results of path planning. In FIG. 11,the obstacle detection unit identifies an obstacle 22 by the cluster ofdata points in the center of the road 80. The path planning unitdetermines that there is adequate clearance to permit the vehicle 10 todeviate to the right as it advances to the obstacle 22 and then deviateleft to return to the center of the road 80. The projected path of thevehicle 10 is shown by path 82. In FIG. 12, the path 82 of the vehicle10 passes a center of the obstacle 22 to the left and then to the rightas shown. The path planning unit has divided the path and the data mapinto sectors as shown. The first sector at the bottom contains noobstacles and requires no deviation along the planned path 82. Thesecond sector contains an obstacle 22 and requires the path planningunit to determine clearance and a path around the obstacle 22. Further,deviation from the path will rely on the speed planning unit to controlthe vehicle so as to safely pass the vehicle at a speed suited for theradius of the turn. FIG. 13 shows a vehicle 10 that is about to navigatea series of obstacles in an immediate path of the vehicle.

The present invention recognizes that smooth paths alone are notnecessarily drivable by the vehicle. The car should be able to slow downproportionally from straight-aways down to hairpin turns. Once a safepath is designed that avoids obstacles and yet remains drivable, thenext step in the planning process is to decide on the speed at which thevehicle should take each section of the path. The speed chosen, in oneembodiment of the present invention, is based on the curvature at thatpoint in the path and upon the curvature further down the path. Thespeed is taken as the minimum of speed for the current curvature and thespeed for future path curvature. The future path curvature is defined bya simple function that multiplies the curvature at a given future pointby a fractional value that decreases towards zero linearly based uponthe distance from the current path point to the given future path point.

The present invention accommodates factors such as vehicle thrust andexternal forces on the vehicle. System identification is a method usedby the present invention by which the parameters that define a systemcan be determined by relating input signal into a system with thesystem's response to develop a transfer function that behaves in thesame way (or a similar way) as the actual vehicle system. For instance,when attempting to control the speed of a vehicle, the inputs are thebrake and accelerator position and the output is the vehicle's speed. Ifit is assumed that the transfer function, H(s), is first-order, it canbe written as

y(s)=H(s)u(s)  (8)

where H(s) is the transfer function of a system, u(s) is the input tothe system, and y(s) is the output from the system. Systemidentification was applied to real world data from the vehiclepropulsion system to arrive at a transfer function of the vehicle systemfor example empirically tested for the working vehicle until confidencein an accurate transfer function was obtained.

Accordingly, speed control of the vehicle, according to the presentinvention, accommodated not only accelerator and brake functions butalso accommodated many other factors in the physical engine system. Forinstance, since the working vehicle had a gas-electric hybrid engine,the coupling of the two propulsion systems was controlled by aninaccessible factory-installed on-board computer tuned for fuelefficiency. Consequently, the mapping of the requested pedal positionand the actual position achieved was not linear and had to be remappedin software by empirical determination. In one embodiment of the presentinvention, the speed of the vehicle is controlled by an integratedproportional-derivative (PD) controller. This controller bases itsoutput on the previous output and on the current error and derivative ofthe error. In the time domain, the controller can be written as

u(t2)=(t2−t1)(K _(p) e(t ₂)+K _(d) e′(t ₂))+u(t ₁)  (9)

where K_(p) and K_(d) are tunable coefficients, u(t) is the output ofthe controller at time t, and e(t) is the error at time t. The error wasdefined as actual output subtracted from target output. Actual outputwas reported by the RT3000™, and target speed was derived from the pathplanning algorithms.

The integrated PD controller was designed and tuned against the derivedtransfer function detailed above. For instance, the weights (for theproportionate control in the PD controller) needed for optimalperformance were derived initially against the computational model ofthe derived transfer function, and then tuned when operated on thevehicle.

Accelerator and steering wheel control was achieved in the workingvehicle using two separate processes, which were both independent of thepath-planning systems. Once a path was decided on by the path-planningalgorithms, acceleration and steering is used exclusively to remain onthe chosen path. Since paths are checked for feasibility upon creation,it was assumed by the control systems that all paths given are possiblefor the vehicle to achieve. In this manner (although the presentinvention can use other starting assumptions), it becomes the burden ofthe control systems to decide how best to proceed in order to follow theselected path.

The steering controller for the working vehicle was a lead-lagcontroller based on the classical single-track model or bicycle modeldescribed ed by Riekert and Schunck “Zur fahrmechanik des gummibereiftenkraftfahrzeugs,” in Ingenieur Archiv, vol. 11, 1940, pp. 210-224, theentire contents of which are incorporated herein by reference. A leadcompensator increases the responsiveness of a system; a lag compensatorreduces (but does not eliminate) the steady state error. The lead-lagcompensator was based on the frequency response of the system. Thelead-lag compensator was similar to that described by D. Bernstein, Astudents guide to classical control, IEEE Control Systems Magazine, vol.17, pp. 96-100 (1997), the entire contents of which are incorporatedherein by reference. The resulting controller in the working vehicle wasa convolution of the two lead and lag functions multiplied by the lowfrequency gain, which was 0.045. Adaptive estimation parameters wereused. Adaptive estimation uses a set of values (parameters) first gainedfrom applying the theoretical functions and then iteratively tests andmodifies the parameters in real-world scenarios (e.g., deep sand, roughterrain, and other terrain types) until the parameters are perfected.

$\begin{matrix}{{F_{lead}(s)} = \frac{{850\mspace{14mu} s} + 1}{{900\mspace{14mu} s} + 1}} & (10) \\{{F_{lag}(s)} = \frac{{2\mspace{14mu} s} + 4}{{0.2\mspace{14mu} s} + 1}} & (11)\end{matrix}$

The discretized controller was implemented as shown in (12) and (13),where x is the state vector, {dot over (x)} is the derivative of thestate vector with respect to time, u is the input vector and δ_(f) isthe output steering angle as measured at the tire with respect to thecenterline. The state vector x is defined as [y_(s) Ψ] where y_(s)refers to the distance from the virtual sensor to the reference path andΨ is the vehicle's yaw rate. The virtual sensor is a point projected agiven distance ahead of the vehicle along the vehicle's centerline. Thispoint is commonly referred to as the look-ahead point, and the distancefrom the look-ahead point to the RT3000 is referred to as the look-aheaddistance.

$\begin{matrix}{\overset{.}{x} = {{\begin{bmatrix}0.90475 & {- 0.00054} \\0.00054 & 0.99998\end{bmatrix}x} + {\begin{bmatrix}{- 1.07538} \\0.00277\end{bmatrix}u}}} & (12)\end{matrix}$δ_(f)=[0.02150 0.00005]x+[0.00005]u  (13)

The input vector u to the controller is defined as [y_(s)]. The outputof the controller is the steering angle measured at the tire withrespect to the centerline.

The steering and control system in the working vehicle assumed that therelationship between the steering wheel angle and the resulting tireangle was linear and that the location of the vehicle's center ofgravity was at the midway point between the front and rear axles. As ameasure of safety the magnitude of the y_(s) signal was monitored toprevent the vehicle from becoming unstable. If y_(s) were to cross agiven threshold, meaning the vehicle is severely off path, the speed ofthe vehicle was reduced to 2 mph. This allowed the vehicle to returnonto the desired path and prevented a possible rollover.

Application Areas

The present invention has widespread application in both autonomouslydriven and human driven vehicles where the present invention functionsrespectively in a primary or secondary mode.

In general, the present invention with various of the attributesdescribed above can be included on a variety of driveable units. Suchunits in one embodiment of the present invention are powered land orwater based vehicles, which include for example automobiles, trucks,sport utility vehicles, armored vehicles, boats, ships, barges, tankers,and armored vessels. For watercraft, the present invention could be usednot only when navigating in weather or nighttime conditions wherevisibility is limited and avoidance of other watercraft is desired, butalso in docking and lock operations where control of the watercraftbetween dock and lock structures is important to minimize damage to thewatercraft of the dock and lock structures.

The present invention also has application to aircraft. In particular,the application to high speed aircraft will depend on detectorsensitivity and accuracy to determine the existence of objects at asufficient distance from the plane that the plane can take correctiveaction. However, airport approach and takeoff where the speeds are nothigh the present invention has utility. For example, on take off andlanding, one concern has been whether flocks of birds frequently at theend of the run way area pose risk to engines. Birds are hard to see atdistance for the human eye and hard to detect by aircraft radar.Moreover, the present invention has application to helicopters,especially rescue helicopters that frequently land in make-shift areaswith obstacles many times hidden from the view of the pilot.

Other application areas include lighter-than-air vehicles (e.g.,autonomous weather balloons, autonomous border patrol system includingfor example remote control small aircraft), other small reconnaissanceaircraft, and amphibious vehicles (such as for example land-sea assaultvehicles including hovercrafts etc.).

In one embodiment of the present invention, the driveable unit can be anautonomous vehicle without driver-assisted control or adriver-controlled vehicle with computer-assisted control. The autonomousvehicles find application, according to one embodiment of the presentinvention, in the above noted environmentally dangerous surroundingswhere a driver would be at risk. The driver-controlled vehicles of thepresent invention find application in the above noted more conventionalenvironments, where the driver may become disoriented or incapable ofphysically commanding the vehicle as would occur if the driver suffereda medical emergency or if for example the driver became disorientedunder an adverse driving condition. Accordingly, in one embodiment ofthe present invention, processor 24 is configured to control the vehiclein an event of driver impairment or in an event of immediate pathobstruction or in an event of driver request.

As an illustrative example of this embodiment of the present invention,the autonomous vehicle could recognize driver impairment throughobserving where the driver is driving in comparison to a predeterminedpath. If the present path of the vehicle and the predetermined path arenot similar, the autonomous vehicle could then check, for example, tosee if the driver has recently turned the steering wheel and/or pressedon the either the brakes or throttle. Both of the comparison and thesteering and brake check can be included in the decision making process,because if the driver were driving on a long straight road with cruisecontrol on, he may not be actively turning the steering wheel orapplying brake or throttle. By the same logic, the driver could bedriving a path that is not consistent with the autonomous vehicle's pathas long as the driver is actively steering the car. In the event thatthe autonomous vehicle needs to take over, then the autonomous vehiclein one embodiment of the present invention first audibly and visiblywarns the driver that it is taking over, then second takes over andsteers the vehicle to a safe stopping position as smoothly and safely aspossible. If the driver wanted to regain control, the autonomous vehicleof the present invention, in one embodiment, provides a press-button orother input device for the driver to resume control. In anotherembodiment of the present invention, the driver could press the button(or command input) again to relinquish control to the autonomousvehicle. Hence, the present invention provides in various embodiments acooperative autonomous driving mode.

In another embodiment of the present invention, a biometric sensor couldrepresent another input device. In this embodiment, the biometric sensordetermines if the driver was actively driving through input to processor24 from for example a heart rate monitor built into the steering wheelof the vehicle. One example of a heart rate monitor that is suitable forthe present invention is heart rate used in exercise equipment, which inone embodiment would be integrated into the steering wheel oralternatively could be worn by the driver and be in wirelesscommunication to processor 24. If the processor 24 detected either acomplete loss of heart rate or an extremely low heart rate for anextended period of time (for example 5 seconds), the processor 24 isconfigured to take control of the vehicle. The processor, by monitoringthe normal heart rate of the driver when the car was under propercontrol would have a basis for determining for example if the driver wasimpaired due to haven fallen asleep at the wheel, having had a stroke,or having had a heart attack. This embodiment could be implemented alsoin the cooperative mode of operation (discussed above). As before, inone embodiment of the present invention, an audible alarm is issuedbefore taking over control of the vehicle and steering the vehicle to asafe stop. If the driver was in fact not impaired, the driver couldsimply press a button (or other input device) to take control back fromthe processor 24.

In another embodiment of the present invention, the autonomous vehiclecan be operated repeatedly over a predetermined course. For instance, ahuman driver presses a button that turns the autonomous vehicle intorecord mode. The human drives the vehicle exactly like he would want theautonomous vehicle to drive the course. The human driver then pressesthe button again, and the autonomous vehicle drives the recorded courseover and over again with a very high level of reliability andrepeatability. (Repeatability is an issue for automobile testers). Thiscapability is also useful for endurance testing vehicles, where thevehicle is driven offroad in hazardous conditions for days in a row inwhich currently many human drivers are used to perform this task due tothe human body's relative weakness. This capability is also useful fordriving a vehicle at consistent speeds for long distances. For example,this capability would be useful in testing a vehicle at highway speedson a race track for fuel consumption performance.

In another embodiment of the present invention, the driveable unit canbe used, in conjunction with a mapping program (e.g., running on alaptop), in which the user could select a destination. At this point,the autonomous navigation software would access the mapping software andgenerate a route (like the software normally does) except in GPSwaypoints rather than in human directions like “turn left at mainstreet.” At this point, the autonomous vehicle proceeds in normaloperation to follow that route. In one embodiment, the mapping softwareis customized to provide additional information to the autonomousnavigation program such as the width of streets and speed limits.

Other application areas for the navigation and control system of thepresent invention include, but are not limited to: 1) agriculturalequipment (e.g., gardening equipment) that performs repetitive tasks ortasks on predicted paths such as for example the harvesting of cropsfrom fields, plowing, grass cutting, etc., 2) mining equipment includingfor example powered carts that could transport for example goods orpeople through blackout or smoke filled passages that would normallyprevent escape, 3) cave exploration equipment, 4) emergency or policevehicles such as for example fire fighting, ambulances, and rescuesquads where visibility impairment need not prevent the emergencyvehicles from proceeding forward (as discussed below) or vehiclesoperating in hazardous environmental conditions where manning of thevehicle places the driver at risk, 5) warehouse management equipmentused to store/retrieve pallets, boxes, etc., and 6) toys.

As an illustrative example of the application of the autonomous vehicleof the present invention to a police vehicle, on the Causeway bridge inNew Orleans (the longest bridge in the world at 24 miles long), there isa significant fog season. On mornings with a dense fog, traffic isconvoyed across the bridge by a police car driving at 35 mph. The lowspeed is required due to the extremely short visibility. On morningswith intense fog, the bridge is closed as even convoying is notpossible. If the lead police car in the convoy were an autonomousvehicle of the present invention operating in the above-notedcooperative mode, the police car could safely convoy in any type of fog,especially when the RIEGL™ imaging sensor is used. The same applies todriving at night. The autonomous vehicle of the present invention is notaffected by darkness.

Numerous modifications and variations on the present invention arepossible in light of the above teachings. It is, therefore, to beunderstood that within the scope of the accompanying claims, theinvention may be practiced otherwise than as specifically describedherein.

1. A land-based drivable unit comprising: a vehicle for groundtransportation including, a sensor configured to locate objects in apredetermined field of view from a vehicle, including, an emitterconfigured to scan a beam into a sector of a plane defined with respectto a first predetermined axis of the vehicle, a detector configured todetect a reflection of the emitted beam from one of the objects, and apanning mechanism configured to pan the plane in which the beam isscanned about a second predetermined axis to produce a three dimensionalfield of view, and a processor configured to determine the existence andlocation of the objects in the three dimensional field of view based onthe three dimensional field of view.
 2. The unit of claim 1, wherein theprocessor determines the existence and location of the objects based ona position of the vehicle and a time between an emittance of the beamand a reception of the reflection of the emitted beam.
 3. The unit ofclaim 1, wherein the vehicle comprises at least one of an automobile, atruck, a sport utility vehicle, rescue vehicle, an agricultural vehicle,a mining vehicle, an escort vehicle, a toy vehicle, a reconnaissancevehicle, a test-track vehicle, and an armored vehicle.
 4. The unit ofclaim 1, wherein the vehicle comprises an autonomous vehicle withoutdriver-assisted control.
 5. The unit of claim 1, wherein the vehiclecomprises a driver-controlled vehicle with computer-assisted control. 6.The unit of claim 5, wherein the processor is configured to recognizedriver impairment.
 7. The unit of claim 6, wherein the processor isconfigured to recognize driver impairment from a biometric sensor orfrom an analysis of driver-control of the vehicle.
 8. The unit of claim5, wherein the processor is configured to control the vehicle in anevent of an input command indicating driver impairment.